#include "factor_graph.h"
#include "math.h"

#define M_PI		3.14159265358979323846

data_t get_distance(data_t x1, data_t y1, data_t x2, data_t y2) {
    double dx = x2 - x1;
    double dy = y2 - y1;
    return sqrt(dx * dx + dy * dy);
}

data_t gaussian_pdf(data_t x, data_t mean, data_t std_dev){
	data_t exponent = -1 * (x - mean) * (x - mean) / (2 * std_dev * std_dev);
	data_t coefficient = 1 / (std_dev * sqrt(2 * M_PI));
	return coefficient * exp(exponent);
}

#define CARD 10

// the priori factor node for UWB sensor
// given station location and distance measurement update the pmf of agent
// station[] = {station_x, station_y};
// z = measurement of UWB sensor
// pmf[] = {dim0 = x, dim1 = y}, the array to output
// x_range[] = {min_x, max_x}
// y_range[] = {min_y, max_y}
void UWB_Factor(data_t station[2], data_t z,  data_t x_range[2], data_t y_range[2], data_t pmf[CARD*CARD]){
        int i, j;
        data_t dx = (x_range[1] - x_range[0]) / (CARD - 1);
        data_t dy = (y_range[1] - y_range[0]) / (CARD - 1);
        data_t x, y;
        data_t probability;
        data_t dis;
        data_t pmf_sum=0.0f;
        for_x_dim:
        for(i = 0; i < CARD; i++){
                x = x_range[0] + i * dx;
                for_y_dim:
                for(j = 0; j < CARD; j++){
                        y = y_range[0] + j * dy;
                        dis = get_distance(station[0], station[1], x, y);
                        probability = gaussian_pdf(z, dis, 0.33);
                        pmf[i*CARD+j] = probability;
                        pmf_sum+=probability;
                }
        }

        normalize:
        for(i = 0; i < CARD*CARD; i++){
                        pmf[i] /= pmf_sum;
        }
        return;
}

#define SIZE 100  // the size of message pow(CARD, 2)

void UWB_Variable(data_t channelOUT[SIZE], data_t channel1[SIZE], data_t channel2[SIZE], data_t channel3[SIZE], data_t channel4[SIZE]){
	data_t sum = 0.0f;
	data_t temp = 0.0f;
	data_t pro12 = 1.0f;
	data_t pro34 = 1.0f;
	// product the message in
product:
	for(int i = 0; i < SIZE; i++){
		pro12 = channel1[i]*channel2[i];
		pro34 = channel3[i]*channel4[i];
		channelOUT[i] = pro12 * pro34;
		sum += channelOUT[i];
	}

	// normalize
normalize:
	for(int i = 0; i < SIZE; i++){
		temp = channelOUT[i] / sum;
		channelOUT[i] = temp; 
	}

	return;
}

